/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    Control_Node.h
 * @author   LJY
 * @brief
 ******************************************************************************
 */
#ifndef CONTROL_NODE_H
#define CONTROL_NODE_H

/* Includes ------------------------------------------------------------------*/
#include "ros/ros.h"
#include "robot/RobotRunner.h"
#include "srpq_controller/SRPQ_Controller.h"
#include "common/LegData.h"
#include "common/RobotCommand.h"
#include "common/RCCommand.h"
#include "common/CheaterState.h"
#include "common/TouchForce.h"
#include "common/ImuData.h"
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/Int8.h"
#include "geometry_msgs/Point.h"

#include <math.h>
#define Ang2Deg 0.0174

/* Private macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
typedef enum {
    Graph_Type1 = 0,
    Graph_Type2,
    Graph_Type3,
    Graph_Type4,
} Debug_GraphType;

typedef enum {
    Graph_Num1 = 0,
    Graph_Num2,
    Graph_Num3,
} Debug_NumType;

typedef struct Route_Cmd {
    float x;
    float y;
    float pro_yaw;
    float yaw;
} Route_Cmd;

typedef struct Route_Data{
    float type;
    float data[3];
} Route_Data;

/* Exported function declarations --------------------------------------------*/
class Route_Planning {
public:
    Route_Planning(RobotRunner* robotRunner, ros::NodeHandle* _n) : _robotRunner(robotRunner), n(_n) {
        current_state = Vec3<float>::Zero();
        target_state = Vec3<float>::Zero();
        State_Puber = n->advertise<sensor_msgs::JointState>("/quadruped/control/state_data", 10);
    }
    ~Route_Planning() {}
    void run(geometry_msgs::Point& loc);   
    void addRouteData(Route_Data data){
        routes_data.push(data);
    }
    float getYaw(void){return _yaw;}
    sensor_msgs::JointState* getRouteState(void) { return &route_state; }
    void updateYaw(float yaw){
        static float last = yaw;
        float pro_yaw = yaw + round * 2 * M_PI;
        if(pro_yaw - last > M_PI) round -= 1;
        else if(pro_yaw - last < -M_PI) round += 1;
        pro_yaw = yaw + round * 2 * M_PI;
        last = pro_yaw;
        _yaw = pro_yaw;
    }
private:
    RobotRunner* _robotRunner;
    ros::NodeHandle* n;
    ros::Publisher State_Puber;
    std::queue<Route_Data> routes_data;
    std::queue<Route_Cmd> routes_cmd;
    Route_Cmd target;
    sensor_msgs::JointState route_state;
    Vec3<float> current_state;
    Vec3<float> target_state;
    float MAX_ERR_X = 0.1;
    float MAX_ERR_Y = 0.1;
    float MAX_ERR_YAW = 0.08;
    float _yaw;
    float round = 0;
    Mat4<float> T;
    uint8_t process_flag = 2;    // 0 位置逼近 1 角度逼近  2 完成
    void calculateTransitionMatrix(Mat3<float> R, Vec3<float> P);
    void addTarget(float* data, uint8_t type);    // 0 直线  1 曲线
    float limit(float in, float min, float max);
    void updateRouteState(Vec3<float> current, Vec3<float> target) {
        route_state.position.clear();
        for (int i = 0; i < 3; i++)
            route_state.position.push_back(current(i));
        for (int i = 0; i < 3; i++)
            route_state.position.push_back(target(i));
        State_Puber.publish(route_state);
    }
    float correctTargetYaw(float yaw) {
        return yaw + round * 2 * M_PI;
    }
};

class Control_Node {
public:
    Control_Node(ros::NodeHandle* _n, bool _sim) : n(_n), sim(_sim) { Node_Init(); }
    void Run(void);
    void updateRobotCommand(const RobotCommand* cmd);
    void Draw_debugData(Debug_GraphType graph_type, Debug_NumType graph_num, float value, std::string name);

private:
    ros::NodeHandle* n;
    ros::Publisher RobotCmd_Puber;
    ros::Publisher AUcmd_Puber;
    ros::Publisher debugData_Puber;
    ros::Publisher State_Puber;
    ros::Subscriber LegData_Suber;
    ros::Subscriber RCCmd_Suber;
    ros::Subscriber CheaterState_Suber;
    ros::Subscriber ImuData_Suber;
    ros::Subscriber TouchForce_Suber;
    ros::Subscriber Nav_Suber;
    ros::Subscriber Route_Suber;
    ros::Subscriber AUCmd_Suber;
    ros::Subscriber Loc_Suber;
    RobotRunner* _robotRunner;
    Route_Planning* planner;
    bool sim = true;    // 控制开启仿真还是实体控制
    bool LegData_Available = false;
    bool CheaterState_Available = false;
    bool RCCmd_Available = false;
    bool ImuData_Available = false;
    bool Estimator_Available = false;
    bool AU_Cmd_Enable = false;
    uint8_t Ctrl_Mode = 0;
    sensor_msgs::JointState DebugData;
    ros::Timer freq_timer;
    ros::Timer debug_timer;
    uint64_t iter = 0;
    uint64_t freq_count = 0;
    uint8_t _au_cmd;
    geometry_msgs::Point robot_loc;
    bool legdataUpdate = false;

    void LegData_Callback(const common::LegDataConstPtr& msg);
    void RCCmd_Callback(const common::RCCommandConstPtr& msg);
    void CheaterState_Callback(const common::CheaterStateConstPtr& msg);
    void TouchForce_Callback(const common::TouchForceConstPtr& msg);
    void ImuData_Callback(const common::ImuDataConstPtr& msg);
    void Navigation_Callback(const geometry_msgs::TwistConstPtr& msg);
    void RoutePlan_Callback(const sensor_msgs::JointState& msg);
    void AUCmd_Callback(const std_msgs::Int8ConstPtr& msg);
    void Location_Callback(const geometry_msgs::PointConstPtr& msg);
    void Node_Init(void);
    void FreqTimer_Callback(const ros::TimerEvent& event);
    void DebugTimer_Callback(const ros::TimerEvent& event);
};

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
